#pragma once

#include "Aircraft.h"
#include <StandardCplusplus.h>
#include <vector>

typedef struct {
	double lat,lon,alt,roll,pitch,heading,speed;
} Stelem;

class Coms
{
private:
    bool packet;
    unsigned char packetXOR, packetID, packetSize;
	int dataAt;
    char data[255];
	bool oldAPen;
    
    Aircraft *aircraft;
	
	void completePacket(uint8_t *p,int id,int size);
	bool pidCfg(int &offset,PID &pid);
	void writeInt(uint8_t *p,int num);
	void writeDouble(uint8_t *p,double num);
	double readDouble(int pos);
	
	unsigned long lastTx;
    
	//INPUT
    void in_eStop();
    void in_changeMode();
	void in_req_planeMode();
    void in_unknown();
	void in_sensors();
	void in_telem();
	void in_systemStatus();
	void in_servoConfig();
	void in_pidConfig();
	void in_servoRead();
	void in_pidRead();
	void in_servo();
	void in_gps();
	void in_setpointRead();
	void in_setpointChange();
	
	//OUTPUT
	void out_ack();//0
	void out_err(int prob,int index);//1
	void out_err(int prob);//1
	void out_err_byte(int byteNumber);//1
	void out_err_size();//1
	void out_planeMode();//2 ==R==
	void out_sensors();//3 ==R==
	void out_telem();//7 ==R==
	void out_systemStatus();//8 ==R==
	void out_servoCalib();//12 ==R==
	void out_pid();//13 ==R==
	void out_servo();//14 ==R==
	void out_gps();//15 ==R==
	void out_setpoints();//16 ==R==
    
public:
	std::vector<uint8_t> registered;
	Coms();
    void init(Aircraft *aircraft);
    void update();
};
